#include "Drive.h"

Drive::Drive(UINT32 joystickChannel, UINT32 leftMotorChannel, UINT32 rightMotorChannel)
{
	
	leftMotor = new Victor(leftMotorChannel);
	rightMotor = new Victor(rightMotorChannel);
	xbox = new Joystick(joystickChannel);
	//js2 = new Joystick(rightJoystickChannel);
}
	
void Drive::drive()
{
	leftVal = Limit(-(xbox->GetRawAxis(2)));
	rightVal = Limit(-(xbox->GetRawAxis(5)));	
	leftVal = leftVal / 2;
	rightVal = rightVal / 2;
	if(xbox->GetRawButton(1))
	{
		this->setMotors(1.0, -1.0);
	}
	else
	{
		this->setMotors(leftVal, -1.0 * rightVal);
	}
}

void Drive::setMotors(float passLeft, float passRight)
{
	leftMotor -> Set(passLeft);
	rightMotor -> Set(passRight);
}

float Drive::getLeftVal() const
{
	return leftMotor->Get();
}

float Drive::getRightVal() const
{
	return rightMotor->Get();
}


void Drive::HaloDrive()
{
	float speed;
	if(xbox->GetTrigger())
	{
		speed = Limit(xbox->GetRawAxis(2));
	}
	else
	{
		speed = Limit(xbox->GetRawAxis(2)) / 2;
	}
	float turn = Limit(xbox->GetRawAxis(4)) / 2;
	if(speed > 0)
	{
		if(turn < 0)
		{
			setMotors(speed + turn, -1.0 * (speed - turn));
		}
		else if(turn > 0)
		{
			setMotors(speed + turn, -1.0 * (speed - turn));
		}
		else
		{
			setMotors(speed, -1.0 * speed);
		}
	}
	else if(speed < 0)
	{
		if(turn < 0)
		{
			setMotors(speed - turn, -1.0 * (speed + turn));
		}
		else if(turn > 0)
		{
			setMotors(speed - turn, -1.0 * (speed + turn));
		}
		else
		{
			setMotors(speed, -1.0 * speed);
		}
	}
	else
	{
		setMotors(0.0, 0.0);
	}
}
float Drive::Limit(float num)
{
	if (num > 1.0)
	{
		return 1.0;
	}
	if (num < -1.0)
	{
		return -1.0;
	}
	return num;
}
Drive::~Drive()
{
	delete leftMotor;
	delete rightMotor;
	delete xbox;
}
